/*
 * nxt.c
 *
 *  Created on: Mar 14, 2012
 *      Author: Victor Nidens victornlj@gmail.com
 *
 *	This file is part of API LEGO for CMLab based on LEGO NXT Direct Commands API
 *	by Michael Collins michaelcollins@ivorycity.com and
 *	Aran Deltac aran@arandeltac.com
 *
 *	API LEGO for CMLab is free software: you can redistribute it and/or modify
 *	it under the terms of the GNU Lesser General Public License as published by
 *	the Free Software Foundation, either version 3 of the License, or
 *	(at your option) any later version.
 *
 *	API LEGO for CMLab library is distributed in the hope that it will be useful,
 *	but WITHOUT ANY WARRANTY; without even the implied warranty of
 *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *	GNU Lesser General Public License for more details.
 *
 *	You should have received a copy of the GNU Lesser General Public License
 *	along with API LEGO for CMLab. If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "nxt.h"
#include "usbcomm.h"
#include "string.h"
#include "stdio.h"
#include "stdlib.h"

int8 nxtParseGenericResponse(uint8 *responseBuffer, uint8 responseBufferLength, GenericResponse *response){

	if(!responseBuffer || responseBufferLength < 3){
		return 1;
	}

	if(!response){
		return 2;
	}

	memset(response, 0, sizeof(GenericResponse));

	memcpy(response, responseBuffer, sizeof(GenericResponse));

	return response->status;

}

int8 nxtParseBatteryStatus(uint8 *responseBuffer, uint8 responseBufferLength, BatteryState *state){

	if(!responseBuffer || responseBufferLength < 5){
		return 1;
	}

	if(!state){
		return 2;
	}

	memset(state, 0, sizeof(BatteryState));

	memcpy(state, responseBuffer, sizeof(BatteryState));

	return state->status;

}

int8 nxtParseMotorOutput(uint8 *responseBuffer, uint8 responseBufferLength, MotorOutputState *output){
	if(responseBufferLength < 25){
		return 1;
	}

	if(!responseBuffer || !output){
		return 2;
	}

	memset(output, 0, sizeof(MotorOutputState));

	memcpy(output, responseBuffer, sizeof(MotorOutputState));

	return output->status;

}

int8 nxtParseSensorState(uint8 *responseBufer, uint8 responseBufferLength, SensorState* state){
	if(responseBufferLength < 15){
		return 1;
	}

	if(!responseBufer || !state){
		return 2;
	}

	memset(state, 0, sizeof(SensorState));

	memcpy(state, responseBufer, sizeof(SensorState));

	return state->status;

}

int8 nxtParseDeviceSystemInfo(uint8 *responseBuffer, uint8 responseBufferLength, DeviceSystemInfo *info){
	if(responseBufferLength < 32){
		return 1;
	}

	if(!responseBuffer || !info){
		return 2;
	}

	memset(info, 0, sizeof(DeviceSystemInfo));

	memcpy(info, responseBuffer, sizeof(DeviceSystemInfo));

	return info->status;

}

int8 nxtParseDigitalSersorStatus(uint8 *responseBuffer, uint8 responseBufferLength, DigitalSensorStatusRespose *response){
	if(responseBufferLength < 3)
		return 1;

	if(!responseBuffer || !response)
		return 2;

	memset(response, 0, sizeof(DigitalSensorStatusRespose));

	memcpy(response, responseBuffer, sizeof(DigitalSensorStatusRespose));

	return response->status;

}

int8 nxtParseDigitalSersorRead(uint8 *responseBuffer, uint8 responseBufferLength, DigitalSensorReadRespose *response) {
	if (responseBufferLength < 20)
		return 1;

	if (!responseBuffer || !response)
		return 2;

	memset(response, 0, sizeof(DigitalSensorReadRespose));

	memcpy(response, responseBuffer, sizeof(DigitalSensorReadRespose));

	return response->status;
}

int8 nxtKeepAlive(libusb_device_handle **deviceHandle, uint8 wantResponse, GenericResponse *response){

	int8 ret = 0;
	uint8 request[3] = {0};
	uint8 responseBuffer[8] = {0};
	int16 responseLength = 8;

	if (*deviceHandle == NULL) {
		return 4;
	}

	if (wantResponse && !response) {
		return 5;
	}

	if(wantResponse){
		request[0] = NXT_RET;
	} else {
		request[0] = NXT_NORET;
	}

	request[1] = NXT_KEEP_ALIVE;

	ret = executeCommand(deviceHandle, request, 2, wantResponse, responseBuffer, &responseLength);

	if(wantResponse && responseLength > 0){
		ret = nxtParseGenericResponse(responseBuffer, responseLength, response);
	}

	return ret;
}

int8 nxtPlaySound(libusb_device_handle **deviceHandle, uint16 tone, uint16 duration, uint8 wantResponse, GenericResponse *response){

	int8 ret = 0;
	uint8 request[6] = {0};
	uint8 responseBuffer[4] = {0};
	int16 responseLength = 4;

	if(*deviceHandle == NULL){
		return 4;
	}

	if(wantResponse && !response){
		return 5;
	}

	if(wantResponse){
		request[0] = NXT_RET;
	} else {
		request[0] = NXT_NORET;
	}

	request[1] = NXT_PLAY_TONE;
	request[2] = tone & 0xff;
	request[3] = (tone & 0xff00) >> 8;
	request[4] = duration & 0xff;
	request[5] = (duration & 0xff00) >> 8;

	ret = executeCommand(deviceHandle, request, 6, wantResponse, responseBuffer, &responseLength);

	if (wantResponse && responseLength > 0) {
		ret = nxtParseGenericResponse(responseBuffer, responseLength, response);
	}

	return ret;

}

int8 nxtStartProgram(libusb_device_handle **deviceHandle, uint8 *file){
	int8 ret = 0;
	uint8 request[32] = {0};

	if(*deviceHandle == NULL){
		return 4;
	}

	if(file == NULL){
		return 5;
	}

	request[0] = NXT_NORET;
	request[1] = NXT_START_PROGRAM;

	sprintf(request + 2, "[%s]", file);

	ret = executeCommand(deviceHandle, request, strlen(request+2)+2, 0, NULL, NULL);

	return ret;
}

int8 nxtGetBatteryLevel(libusb_device_handle **deviceHandle, BatteryState *battery){
	int8 ret = 0;
	uint8 request[3] = {0};
	uint8 responseBuffer[8] = {0};
	int responseLength = 8;

	if (*deviceHandle == NULL) {
		return 4;
	}

	if (battery == NULL) {
		return 5;
	}

	memset(battery, 0, sizeof(BatteryState));

	request[0] = NXT_RET;
	request[1] = NXT_GET_BATTERY_LEVEL;

	ret = executeCommand(deviceHandle, request, 2, 1, responseBuffer, &responseLength);

	if(responseLength > 0){
		ret = nxtParseBatteryStatus(responseBuffer, responseLength, battery);
	}

	return ret;

}

int8 nxtMotorsRotate(libusb_device_handle **deviceHandle,
						uint8 motorPort,
						int8 power,
						uint8 mode,
						uint8 regulationMode,
						int8 turnRatio,
						uint8 runstateMode,
						uint32 tachoLimit,
						uint8 wantResponse,
						GenericResponse *response){

	int8 ret = 0;
	uint8 request[10] = {0};
	uint8 responseBuffer[4] = {0};
	int responseLength = 4;


	if (*deviceHandle == NULL) {
		return 4;
	}

	if (wantResponse && !response) {
		return 5;
	}

	if (wantResponse) {
		request[0] = NXT_RET;
	} else {
		request[0] = NXT_NORET;
	}

	request[1] = NXT_SET_OUTPUT_STATE;
	request[2] = motorPort;
	request[3] = power;
	request[4] = mode;
	request[5] = regulationMode;
	request[6] = turnRatio;
	request[7] = runstateMode;
	memcpy(request + 8, &tachoLimit, 4);

	ret = executeCommand(deviceHandle, request, sizeof(request), 1, responseBuffer, &responseLength);

	if(wantResponse && responseLength > 0){
		ret = nxtParseGenericResponse(responseBuffer, responseLength, response);
	}

	return ret;

}

int8 nxtGetMotorState(libusb_device_handle **deviceHandle, uint8 motorPort, MotorOutputState *motorState){

	int8 ret = 0;
	uint8 request[4] = { 0 };
	uint8 responseBuffer[26] = { 0 };
	int responseLength = 26;

	if (*deviceHandle == NULL) {
		return 4;
	}

	request[0] = NXT_RET;
	request[1] = NXT_GET_OUTPUT_STATE;
	request[2] = motorPort;

	ret = executeCommand(deviceHandle, request, sizeof(request), 1, responseBuffer, &responseLength);

	if(responseLength > 0){
		ret = nxtParseMotorOutput(responseBuffer, responseLength, motorState);
	}

	return ret;

}

int8 nxtGetInputValues(libusb_device_handle **deviceHandle, uint8 port, SensorState *sensorState){
	int8 ret = 0;
	uint8 request[4] = { 0 };
	uint8 responseBuffer[26] = { 0 };
	int responseLength = 26;

	if (*deviceHandle == NULL) {
		return 4;
	}

	if(!sensorState){
		return 5;
	}

	request[0] = NXT_RET;
	request[1] = NXT_GET_INPUT_VALUES;
	request[2] = port;

	ret = executeCommand(deviceHandle, request, sizeof(request), 1,
			responseBuffer, &responseLength);

	if (responseLength > 0) {
		ret = nxtParseSensorState(responseBuffer, responseLength, sensorState);
	}

	return ret;
}

int8 nxtGetDeviceSystemInfo(libusb_device_handle **deviceHandle, DeviceSystemInfo *info){
	int8 ret = 0;
	uint8 request[2] = { 0 };
	uint8 responseBuffer[33] = { 0 };
	int responseLength = 33;

	if (*deviceHandle == NULL) {
		return 4;
	}

	if(!info){
		return 5;
	}

	request[0] = NXT_SYSOP;
	request[1] = NXT_SYS_GET_DEVICE_INFO;

	ret = executeCommand(deviceHandle, request, sizeof(request), 1,
				responseBuffer, &responseLength);

	if (responseLength > 0) {
		ret = nxtParseDeviceSystemInfo(responseBuffer, responseLength, info);
	}

	return ret;

}

int8 nxtSetInputMode(libusb_device_handle **deviceHandle, uint8 port, uint8 sensorType, uint8 sensorMode, uint8 wantResponse, GenericResponse *response){
	int8 ret = 0;
	uint8 request[5] = {0};
	uint8 responseBuffer[4] = {0};
	int responseLength = 4;

	if(*deviceHandle == NULL){
		return 4;
	}

	if(!response){
		return 5;
	}

	if(wantResponse){
		request[0] = NXT_RET;
	} else {
		request[0] = NXT_NORET;
	}

	request[1] = NXT_SET_INPUT_MODE;
	request[2] = port;
	request[3] = sensorType;
	request[4] = sensorMode;

	ret = executeCommand(deviceHandle,
							request, sizeof(request),
							wantResponse,
							responseBuffer, &responseLength);

	if(wantResponse && responseLength > 0){
		ret = nxtParseGenericResponse(responseBuffer, responseLength, response);
	}

	return ret;

}

int8 nxtLSGetStatus(libusb_device_handle **deviceHandle, uint8 port, uint8 wantResponse, DigitalSensorStatusRespose *response){

	int8 ret = 0;
	uint8 request[3] = {0};
	uint8 responseBuffer[4] = {0};
	int responseLength = 4;

	if(*deviceHandle == NULL)
		return 4;

	if(wantResponse && !response)
		return 5;

	if(wantResponse){
		request[0] = NXT_RET;
	} else {
		request[0] = NXT_NORET;
	}

	request[1] = NXT_LSGET_STATUS;
	request[2] = port;

	ret = executeCommand(deviceHandle,
							request, sizeof(request),
							wantResponse,
							responseBuffer, &responseLength);

	if(wantResponse && responseLength > 0)
		ret = nxtParseDigitalSersorStatus(responseBuffer, responseLength, response);


	return ret;

}

int8 nxtLSWrite(libusb_device_handle **deviceHandle, uint8 port, uint8 txlen, uint8 rxlen, uint8 *txdata, uint8 wantResponse, GenericResponse *response){
	int8 ret = 0;
	uint8 request[28] = {0};
	uint8 responseBuffer[4] = {0};
	int responseLength = 4;

	if (*deviceHandle == NULL)
		return 4;

	if (wantResponse && !response)
		return 5;

	if (wantResponse) {
		request[0] = NXT_RET;
	} else {
		request[0] = NXT_NORET;
	}

	request[1] = NXT_LSWRITE;
	request[2] = port;
	request[3] = txlen;
	request[4] = rxlen;
	memcpy(request + 5, txdata, txlen);

	ret = executeCommand(deviceHandle,
							request, 6 + txlen,
							wantResponse,
							responseBuffer, &responseLength);

	if(wantResponse && responseLength > 0)
		ret = nxtParseGenericResponse(responseBuffer, responseLength, response);

	return ret;

}

int8 nxtLSRead(libusb_device_handle **deviceHandle, uint8 port,
		DigitalSensorReadRespose *response) {
	int8 ret = 0;
	uint8 request[3] = { 0 };
	uint8 responseBuffer[21] = { 0 };
	int responseLength = 21;

	if (*deviceHandle == NULL)
		return 4;

	request[0] = NXT_RET;

	request[1] = NXT_LSREAD;
	request[2] = port;

	ret = executeCommand(deviceHandle,
							request, sizeof(request),
							1,
							responseBuffer, &responseLength);

	ret = nxtParseDigitalSersorRead(responseBuffer, responseLength, response);

	return ret;
}

int8 nxtLSRequestResponse(libusb_device_handle **deviceHandle, uint8 port, uint8 txlen, uint8 rxlen, uint8 *data, DigitalSensorReadRespose *readResponse){
	int ret = 0;
	DigitalSensorStatusRespose status;
	GenericResponse generic;

	ret = nxtLSWrite(deviceHandle, port, txlen, rxlen, data, 1, &generic);

	do{
		ret = nxtLSGetStatus(deviceHandle, port, 1, &status);
	} while(status.bytesReady < rxlen);

	ret = nxtLSRead(deviceHandle, port, readResponse);

	return ret;

}

char* nxtGetStatusDescription(uint8 nxtStatus){
	switch(nxtStatus){

	//Direct Command errors
	case 0x20:
		return "Pending communication transaction in progress";
	case 0x40:
		return "Specified mailbox queue is empty";
	case 0xBD:
		return "Request failed (i.e. specified file not found)";
	case 0xBE:
		return "Unknown command opcode";
	case 0xBF:
		return "Insane packet";
	case 0xC0:
		return "Data contains out-of-range values";
	case 0xDD:
		return "Communication bus error";
	case 0xDE:
		return "No free memory in communication buffer";
	case 0xDF:
		return "Specified channel/connection is not valid";
	case 0xE0:
		return "Specified channel/connection not configured or busy";
	case 0xEC:
		return "No active program";
	case 0xED:
		return "Illegal size specified";
	case 0xEE:
		return "Illegal mailbox queue ID specified";
	case 0xEF:
		return "Attempted to access invalid field of a structure";
	case 0xF0:
		return "Bad input or output specified";
	case 0xFB:
		return "Insufficient memory available";
	case 0xFF:
		return "Bad arguments";

	//System errors
	case 0x81:
		return "No more handles";
	case 0x82:
		return "No space";
	case 0x83:
		return "No more files";
	case 0x84:
		return "End of file expected";
	case 0x85:
		return "End of file";
	case 0x86:
		return "Not a linear file";
	case 0x87:
		return "File not found";
	case 0x88:
		return "Handle all ready closed";
	case 0x89:
		return "No linear space";
	case 0x8A:
		return "Undefined error";
	case 0x8B:
		return "File is busy";
	case 0x8C:
		return "No write buffers";
	case 0x8D:
		return "Append not possible";
	case 0x8E:
		return "File is full";
	case 0x8F:
		return "File exists";
	case 0x90:
		return "Module not found";
	case 0x91:
		return "Out of boundary";
	case 0x92:
		return "Illegal file name";
	case 0x93:
		return "Illegal handle";
	default:
		return "";
	}
}
